#pragma once
#include <stdexcept>  
#include <iostream>
#include <Eigen/Eigen>
using namespace std;
typedef Eigen::Matrix4d SO4;
typedef Eigen::Matrix3d SO3;
struct Pose3D
{
public:
	/** X,Y,Z, coords */
	double x{ .0 }, y{ .0 }, z{ .0 };
	/** Yaw coordinate (rotation angle over Z axis). */
	double yaw{ .0 };
	/** Pitch coordinate (rotation angle over Y axis). */
	double pitch{ .0 };
	/** Roll coordinate (rotation angle over X coordinate). */
	double roll{ .0 };
	//** From the 4*4 rotation matrix 
	SO4 m_ROT4;
	//** From the 3*3 rotation matrix 
	SO3 m_ROT3;

	/**
	 * Constructor from coordinates.
	 */
	Pose3D(
		double _x, double _y, double _z, double _yaw, double _pitch,
		double _roll)
		: x(_x), y(_y), z(_z), yaw(_yaw), pitch(_pitch), roll(_roll)
	{
		fromROT3(_yaw, _pitch,_roll);
	}
	void fromROT4();
	void fromROT3(double _yaw, double _pitch, double _roll);
	void fromROT3();
	void fromROT4(double _x, double _y, double _z, double _yaw, double _pitch,double _roll);
	/*
	 ** transformPoint the local point to global point.
	 ** global_point = Rotation(3*3) * local_point + T
	 */
 	void transformPoint(const double& lx, const double& ly, const double& lz, float& gx, float& gy, float& gz)const;
 	
	double distance2Pose(const float& px, const float& py, const float& pz)const;

	double distance2Pose(const Pose3D& pose)const;

	/**
	 * Default fast constructor. Initializes to zeros.
	 */
	Pose3D();
	/**
	 * Default fast constructor. Initializes to zeros.
	 */
	Pose3D(const Pose3D& pose) {
		x = pose.x;
		y = pose.y;
		z = pose.z ;
		yaw = pose.yaw;
		pitch = pose.pitch;
		roll = pose.roll;
		fromROT3();
		fromROT4();
	}
	/**
	 * Default destruct.
	 */
	~Pose3D();
	/** Coordinate access using operator[]. Order: x,y,z,yaw,pitch,roll */
	double& operator[](size_t i)
	{
		switch (i)
		{
		case 0:
			return x;
		case 1:
			return y;
		case 2:
			return z;
		case 3:
			return yaw;
		case 4:
			return pitch;
		case 5:
			return roll;
		default:
			throw std::out_of_range("index out of range");
		}
	}
	friend ostream& operator<< (ostream &out, const Pose3D &pose) {
		out << "pose=" << endl;
		out << pose.x << " " << pose.y << " " << pose.z << " " 
			<< "yaw="<<pose.yaw << " " << "pitch=" << pose.pitch << " " << "roll=" << pose.roll;
		return out;
	}

	friend Pose3D operator+(const Pose3D& a,const Pose3D& b) {



		return Pose3D(a.x+b.x,a.y+b.y,a.z + b.z,
			a.yaw + b.yaw,a.pitch + b.pitch,a.roll + b.roll);
	}
	friend Pose3D operator-(const Pose3D& a, const Pose3D& b) {
		return Pose3D(a.x - b.x, a.y - b.y, a.z - b.z,
			a.yaw - b.yaw, a.pitch - b.pitch, a.roll - b.roll);
	}

};

